#ifndef AGV_OBSTACLE_AVOIDANCE__AVOID_NODE_HPP_
#define AGV_OBSTACLE_AVOIDANCE__AVOID_NODE_HPP_

#include <string>
#include <memory>
#include <tinyxml2.h>
#include "rclcpp/rclcpp.hpp"
#include "agv_srvs/srv/avoid_obsta_switch.hpp"
#include "agv_srvs/srv/avoid_obsta_update.hpp"
#include "agv_msgs/msg/avoid_obsta_info.hpp"
#include "agv_msgs/msg/agv_mode.hpp"
#include "agv_msgs/msg/agv_event.hpp"
#include "agv_msgs/msg/bat_info.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "laser_obsta_avoid.hpp"
#include "io_obsta_avoid.hpp"
#include "depth_camera.hpp"
#include "scenes_judge.hpp"
#include "scan_filter.hpp"

namespace AgvObstaAvoid
{

class AvoidNode : public rclcpp::Node
{
public:
    // Constructor and destructor
    explicit AvoidNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
    ~AvoidNode() = default;

    // Initialization
    void init();
    bool InitAvoidanceParam();
    void update();
    
    bool stop_update_;
private:
    enum LidarNum {
        zero = 0,
        one = 1,
        two = 2,
        three = 3,
        four = 4,
    };

    // Member variables
    bool lidar_node_mon_;
    bool avoid_node_mon_;
    bool agv_work_mode_;
    bool charge_;
    bool agv_state_;
    bool lidar_equip_empty_;
    bool io_equip_empty_;
    bool camera_equip_empty_;
    bool io_enable_;

    // Components
    std::shared_ptr<LaserObstaAvoid> laser_avoid_;
    std::shared_ptr<IoObstaAvoid> io_avoid_;
    std::shared_ptr<ScenesJudge> scenes_judge_;
    std::shared_ptr<ScanFilterChain> scan_filter_;
    std::shared_ptr<DepthCamera> camera_avoid_;

    // Final obstacle avoidance solution
    ScenesJudge::EquipDispatch avoid_result_;

    // ROS2 interfaces
    rclcpp::Publisher<agv_msgs::msg::AvoidObstaInfo>::SharedPtr avoid_pub_;
    rclcpp::Publisher<agv_msgs::msg::AGVEvent>::SharedPtr event_pub_;
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr lidar_sub_1_;
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr lidar_sub_2_;
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laser_sub_1_;
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laser_sub_2_;
    rclcpp::Subscription<agv_msgs::msg::AGVMode>::SharedPtr agv_mode_sub_;
    rclcpp::Subscription<agv_msgs::msg::BatInfo>::SharedPtr battery_sub_;
    rclcpp::Subscription<agv_msgs::msg::OdometryLite>::SharedPtr odom_sub_;
    rclcpp::Subscription<agv_msgs::msg::AgvAutoStatus>::SharedPtr auto_status_sub_;
    rclcpp::Subscription<agv_msgs::msg::PeriphStatus>::SharedPtr peripher_sub_;
    rclcpp::Service<agv_srvs::srv::AvoidObstaUpdate>::SharedPtr update_avoid_file_;
    rclcpp::Service<agv_srvs::srv::AvoidObstaSwitch>::SharedPtr avoid_switch_ser_;
    rclcpp::TimerBase::SharedPtr combined_timer_;
    std::chrono::steady_clock::time_point last_event_time_;
    bool first_callback_ = true;

    // The obstacle avoidance file path
    std::string agv_avoid_file_path_;

    // Callbacks
    void PublishEvent();
    void AgvModeCallback(const agv_msgs::msg::AGVMode::SharedPtr info_msg);
    void BatteryCallback(const agv_msgs::msg::BatInfo::SharedPtr bat_msg);
    void OdomCallback(const agv_msgs::msg::OdometryLite::SharedPtr odom_msg);
    void AutoStatusCallback(const agv_msgs::msg::AgvAutoStatus::SharedPtr status_msg);
    void PeripheralCallback(const agv_msgs::msg::PeriphStatus::SharedPtr peripher_msg);

    // Service handlers
    void HandleAvoidObstaUpdate(
        const std::shared_ptr<agv_srvs::srv::AvoidObstaUpdate::Request> req,
        std::shared_ptr<agv_srvs::srv::AvoidObstaUpdate::Response> resp);
    
    void HandleManualAvoidSwitch(
        const std::shared_ptr<agv_srvs::srv::AvoidObstaSwitch::Request> req,
        std::shared_ptr<agv_srvs::srv::AvoidObstaSwitch::Response> resp);
};

} // namespace AgvObstaAvoid

#endif // AGV_OBSTACLE_AVOIDANCE__AVOID_NODE_HPP_